﻿#pragma once
namespace graph
{
   namespace details
   {
      inline graph_metadata_t::graph_metadata_t(graph_t* graph, bool use_fast_rasterization)
               :  graph_(graph)
               ,  cell_edge_map_d1_(graph->get_edge_count())
               ,  cell_edge_map_d2_(graph->get_edge_count())
               ,  cell_edge_map_d4_(graph->get_edge_count())
               ,  err_count_(0)
               ,  use_fast_rasterization_(use_fast_rasterization)
      {
         init_cell_edge_map();
         init_cell_vertex_map();
      }
      
      //lat = -90 to 90 - I make it 0..180 
      //lon = -180 to 180 - I make it 0..360
      //modular_arithm - no need to cut result at 360 grad
      template<delta d, bool modular_arithm>
      inline cell_id_t graph_metadata_t::get_cell_id(point_t point)
      {         
         cell_id_1dim_t id1;
         cell_id_2dim_t id2;
         long double temp_int_part;
         //lat is y
         //lon is x        
         temp_int_part = ((long double)point.lon_ + 180) / grad_per_cell_traits_t<d>::value();
         std::modf( temp_int_part, &temp_int_part );
         id1 = (cell_id_1dim_t)temp_int_part;
         if (modular_arithm)
            id1 %= max_cell_id<d>().first;
         
         temp_int_part = ((long double)point.lat_ + 90) / grad_per_cell_traits_t<d>::value();         
         std::modf( temp_int_part, &temp_int_part );
         id2 = (cell_id_2dim_t)temp_int_part;
         if (modular_arithm)
            id2 %= max_cell_id<d>().second;
         //checking on fp errors
         //make backward projection and check that point lies in the rectangle
         point_pair_t id_rect = get_cell_rect<d>(cell_id_t(id1, id2));         
         if (
              !((point.lon_ >= id_rect.first.lon_)  &&
               (point.lat_ >= id_rect.first.lat_)  &&
               (point.lon_ <= id_rect.second.lon_) &&
               (point.lat_ <= id_rect.second.lat_)  )
         )
         {
            //there is fp error
            //use gmp or test nearest cells - the target result is one of them
            //here we use nearest cells
            for(int i = -1; i <= 1; ++i)
               for(int j = -1; j <= 1; ++j)
               {
                  id_rect = get_cell_rect<d>(cell_id_t(id1 + i, id2 + j));
                  if (
                      (point.lon_ >= id_rect.first.lon_)  && 
                      (point.lat_ >= id_rect.first.lat_)  &&
                      (point.lon_ <= id_rect.second.lon_) &&
                      (point.lat_ <= id_rect.second.lat_)
                  )
                  {
                     return cell_id_t(id1 + i, id2 + j);
                  }
               }
            throw graph_exception_t("fp error with earth grid couldn't be corrected");
         }
         else 
            return cell_id_t(id1, id2);
      }
      
      template<delta d>
      inline cell_id_t graph_metadata_t::get_cell_id(point_t point)
      {
         return get_cell_id<d, true>(point);
      }
      
      template<delta d, bool modular_arithm>
      inline cell_id_t graph_metadata_t::get_cell_id(mpq_t lat, mpq_t lon)
      {         
         mpq_t loc_lat, loc_lon;
         mpq_inits(loc_lat, loc_lon, 0);
         mpq_set(loc_lat, lat);
         mpq_set(loc_lon, lon);
         mpq_t grad_per_cell, mpq_180, mpq_90;
         mpq_inits(grad_per_cell, mpq_180, mpq_90, 0);
         mpq_set_d(grad_per_cell, grad_per_cell_traits_t<d>::value());
         mpq_set_ui(mpq_180, 180, 1);
         mpq_set_ui(mpq_90, 90, 1);

         cell_id_1dim_t id1;
         cell_id_2dim_t id2;         
         mpz_t mp_id1, mp_id2;
         mpz_init(mp_id1); mpz_init(mp_id2);
         //lat is y
         //lon is x                 
         mpq_add(loc_lon, loc_lon, mpq_180);         
         mpq_div(loc_lon, loc_lon, grad_per_cell);
         mpz_set_q(mp_id1, loc_lon);
         
         mpq_add(loc_lat, loc_lat, mpq_90);
         mpq_div(loc_lat, loc_lat, grad_per_cell);
         mpz_set_q(mp_id2, loc_lat);
         
         id1 = mpz_get_ui(mp_id1);
         if (modular_arithm)
            id1 %= max_cell_id<d>().first;
         id2 = mpz_get_ui(mp_id2);
         if (modular_arithm)
            id2 %= max_cell_id<d>().second;
         
         mpq_clears(grad_per_cell, mpq_180, mpq_90, 0);
         
         
         //checking on fp errors
         //make backward projection and check that point lies in the rectangle
         //but for gmp it is not needed
         //mpq_t lat1, lon1, lat2, lon2;
         //mpq_inits(lat1, lon1, lat2, lon2, 0);
         //get_cell_rect_hp<d>(cell_id_t(id1, id2), lat1, lon1, lat2, lon2);
         //if (
         //     !( ( mpq_cmp(lon, lon1) >= 0  )  &&
         //        ( mpq_cmp(lat, lat1) >= 0   )  &&
         //        ( mpq_cmp(lon, lon2) <= 0 ) &&
         //        ( mpq_cmp(lat, lat2) <= 0)
         //      )
         //   )
         //{
         //   double lon1 = mpq_get_d(lon);
         //   double lat1 = mpq_get_d(lat);
         //   int a = 0;
         //   throw graph_exception_t("fp error with earth grid couldn't be corrected");
         //}
         //mpq_clears(lat1, lon1, lat2, lon2, 0);
         
         mpq_clears(loc_lat, loc_lon, 0);
         mpz_clears(mp_id1, mp_id2, 0);
         return cell_id_t(id1, id2);
      }
      
      template<delta d>
      inline cell_id_t graph_metadata_t::get_cell_id(mpq_t lat, mpq_t lon)
      {
         return get_cell_id<d, true>(lat, lon);
      }
      
      template<delta d>
      inline point_pair_t graph_metadata_t::get_cell_rect(cell_id_t id)
      {
         long double grad_per_cell = grad_per_cell_traits_t<d>::value();
         //lat is y
         //lon is x
         point_t p1( id.second * grad_per_cell - 90, 
                     id.first * grad_per_cell - 180);
         point_t p2( (id.second + 1) * grad_per_cell - 90,
                     (id.first + 1) * grad_per_cell - 180);                     
         return point_pair_t(p1, p2);
      }     
      
      template<delta d>
      inline void graph_metadata_t::get_cell_rect_hp(cell_id_t id, mpq_t lat1, mpq_t lon1, mpq_t lat2, mpq_t lon2)
      {
         mpq_t grad_per_cell, mpq_180, mpq_90;
         mpq_inits(grad_per_cell, mpq_180, mpq_90, 0);
         mpq_set_d(grad_per_cell, grad_per_cell_traits_t<d>::value());
         mpq_set_ui(mpq_180, 180, 1);
         mpq_set_ui(mpq_90, 90, 1);
         
         mpq_set_ui(lat1, id.second, 1);
         mpq_set_ui(lon1, id.first, 1);
         mpq_set_ui(lat2, id.second + 1, 1);
         mpq_set_ui(lon2, id.first + 1, 1);
         
         mpq_mul(lat1, lat1, grad_per_cell);
         mpq_mul(lon1, lon1, grad_per_cell);
         mpq_mul(lat2, lat2, grad_per_cell);
         mpq_mul(lon2, lon2, grad_per_cell);
         
         mpq_sub(lat1, lat1, mpq_90);
         mpq_sub(lon1, lon1, mpq_180);
         mpq_sub(lat2, lat2, mpq_90);
         mpq_sub(lon2, lon2, mpq_180);
      }
      
      //for each edge make its rasterisized representation for every pixel height and width
      inline void graph_metadata_t::init_cell_edge_map()
      {
         edge_iterator_pair_t e_it = edges(*graph_);
         //size_t edge_count = 0;
         for(; e_it.first != e_it.second; ++e_it.first)
         {
            edge_t edge = *e_it.first;
            if (use_fast_rasterization_)
            {
               rasterize_edge<d1>(edge);
               rasterize_edge<d2>(edge);
               rasterize_edge<d4>(edge);
            }
            else
            {
               rasterize_edge2<d1>(edge);
               rasterize_edge2<d2>(edge);
               rasterize_edge2<d4>(edge);
            }
         }        
      }
      
      inline void graph_metadata_t::init_cell_vertex_map()
      {
         //vertex_iterator_pair_t v_it = vertices(*graph_);
         //for(; v_it.first != v_it.second; ++v_it.first)
         //{
         //   vertex_t& v  = *v_it.first;
         //   cell_id_t v_cell_id = get_cell_id<d1>(v.get_point());
         //   cell_vertex_map_d1_.insert(std::make_pair(v_cell_id, v.get_id()));
         //}
      }

      //Better precision then rasterize_edge2
      //because implemented it two ways - float and gmp
      //has correct switching between them, but switching is commented.
      //Because we don't need such precision, but need performance
      //Works with equation of line. Finds intersection x_ of line with 
      //next horizontal line of grid (y). 
      //Iterates through x coord to x_ and finds next intersection of line with (y + step_by_y)
      template<delta d>
      inline void graph_metadata_t::rasterize_edge(edge_t const & edge)
      {
         //for our purposes high precision is not needed
         //to switch it on, define RASTER_HIGH_PREC
         //#define RASTER_HIGH_PREC
         cell_edge_map_t* cur_d_edge_map = &get_cell_edge_map<d>(); //old code was with pointer
         lat_lon_t grad_per_cell = grad_per_cell_traits_t<d>::value();
         lat_lon_t little = grad_per_cell * 0.5;
         lat_lon_t precision_limit = RASTER_PREC_LIMIT;
         
         vertex_t const & source = (*graph_)[edge.get_source_vertex_id()];
         vertex_t const & target = (*graph_)[edge.get_target_vertex_id()];
         lat_lon_t x1 = source.get_point().lon_;
         lat_lon_t x2 = target.get_point().lon_;
         lat_lon_t y1 = source.get_point().lat_;
         lat_lon_t y2 = target.get_point().lat_;  
         
         #ifdef RASTER_TEST
         //graph has dirty edges with veeeery big len
         //filter them to avoid logical failures related to precision
         if ( abs(x1) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(x2) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(y1) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(y2) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 )
            return; 
          #endif
         
         if (x1 > x2)
         {
            std::swap(x1, x2);
            std::swap(y1, y2);
         }
         
         //lat,lon aligned to grid
         lat_lon_t y_min, y_max, x_min, x_max;
         cell_id_t id1 = get_cell_id<d>(point_t(y1, x1));
         cell_id_t id2 = get_cell_id<d>(point_t(y2, x2));
         y_min = get_cell_rect<d>(id1).first.lat_;
         y_max = get_cell_rect<d>(id2).second.lat_;
         x_min = get_cell_rect<d>(id1).first.lon_;
         x_max = get_cell_rect<d>(id2).second.lon_;
         
         #ifdef RASTER_HIGH_PREC
         //switch to high precision
         if ( 
              abs(x1 - x_min) < precision_limit || 
              abs(x1 - x_max) < precision_limit ||
              abs(x2 - x_min) < precision_limit || 
              abs(x2 - x_max) < precision_limit ||
              abs(y1 - y_min) < precision_limit || 
              abs(y1 - y_max) < precision_limit ||
              abs(y2 - y_min) < precision_limit || 
              abs(y2 - y_max) < precision_limit
            )
         {
            rasterize_edge_hp<d>(edge);
         }
         #endif
         
         if (y1 > y2)
         {
            y_min = get_cell_rect<d>(id1).second.lat_;
            y_max = get_cell_rect<d>(id2).first.lat_;
            x_min = get_cell_rect<d>(id1).first.lon_;
            x_max = get_cell_rect<d>(id2).second.lon_;
         }
         lat_lon_t x = x_min;
         lat_lon_t y = y_min;
         lat_lon_t slope = (x2 - x1) / (y2 - y1); //ctg
         
         if (y2 == y1)
         {
            while(x + precision_limit < x2)
            {
               #ifdef RASTER_TEST
                  if (y2 + precision_limit < y_min || 
                      y2 - precision_limit > y_max || 
                      x + little + precision_limit < x_min || 
                      x + little - precision_limit > x_max)
                     throw graph_exception_t("rasterization isn't correct");
               #endif
               cell_id_t new_cell_id = get_cell_id<d>(point_t(y2, x + little));
               cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
               x += grad_per_cell;
               #ifdef RASTER_HIGH_PREC
               //switch to high precision
               if (abs(x - x_max) < precision_limit)
               {
                  rasterize_edge_hp<d>(edge);
                  return;
               }
               #endif
            }
         }
         else
         {
            if (y_min + precision_limit <= y_max) 
            {
              while (y + precision_limit < y2) 
              {
                lat_lon_t x_ = slope * (y + grad_per_cell - y1) + x1;
                //x_ - x coord of intersection with current horizontal axis
                #ifdef RASTER_HIGH_PREC
                if (abs(x_ - x_max) < precision_limit)
                {
                   rasterize_edge_hp<d>(edge);
                   return;
                }
                #endif
                do
                {
                  #ifdef RASTER_TEST
                     if (y + little + precision_limit < y_min || 
                         y + little - precision_limit > y_max || 
                         x + little + precision_limit < x_min || 
                         x + little - precision_limit > x_max)
                        throw graph_exception_t("rasterization isn't correct");
                  #endif
                  cell_id_t new_cell_id = get_cell_id<d>(point_t(y + little, x + little));
                  cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
                  x += grad_per_cell;
                  //switch to high precision
                  #ifdef RASTER_HIGH_PREC
                  if (abs(x - x_) < precision_limit)
                  {
                     rasterize_edge_hp<d>(edge);
                     return;
                  }
                  #endif
                }
                while (x + precision_limit < x_ && x + precision_limit < x2);
                x -= grad_per_cell;
                y += grad_per_cell;
              }
            }
            if (y_min > y_max)
            {
              while (y - precision_limit > y2) 
              {
                lat_lon_t x_ = slope * (y - grad_per_cell - y1) + x1;
                //x_ - x coord of intersection with current horizontal axis
                #ifdef RASTER_HIGH_PREC
                if (abs(x_ - x_max) < precision_limit)
                {
                   rasterize_edge_hp<d>(edge);
                   return;
                }
                #endif
                do
                {
                  #ifdef RASTER_TEST
                  if (y - little - precision_limit < y_max || 
                      y - little + precision_limit > y_min || 
                      x + little + precision_limit < x_min || 
                      x + little - precision_limit > x_max)
                     throw graph_exception_t("rasterization isn't correct");
                  #endif
                  cell_id_t new_cell_id = get_cell_id<d>(point_t(y - little, x + little));
                  cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
                  x += grad_per_cell;   
                  #ifdef RASTER_HIGH_PREC
                  //switch to high precision
                  if (abs(x - x_) < precision_limit)
                  {
                     rasterize_edge_hp<d>(edge);
                     return;
                  }
                  #endif
                }
                while (x + precision_limit < x_ && x + precision_limit < x2);
                x -= grad_per_cell;
                y -= grad_per_cell;
              }
            }
         }   
         #ifdef RASTER_HIGH_PREC    
            #undef RASTER_HIGH_PREC    
         #endif
      }     
      
      //high precision
      template<delta d> 
      inline void graph_metadata_t::rasterize_edge_hp(edge_t const & edge)
      {
         cell_edge_map_t* cur_d_edge_map = &get_cell_edge_map<d>();//old code was with pointer
         mpq_t grad_per_cell;
         mpq_init(grad_per_cell);
         mpq_set_d(grad_per_cell, grad_per_cell_traits_t<d>::value());
                 
         mpq_t little, two;
         mpq_inits(little, two, 0);
         mpq_set_d(two, 2);
         mpq_div(little, grad_per_cell, two);
         mpq_clear(two);
         
         vertex_t const & source = (*graph_)[edge.get_source_vertex_id()];
         vertex_t const & target = (*graph_)[edge.get_target_vertex_id()];
         mpq_t x1, x2, y1, y2;
         mpq_inits(x1, x2, y1, y2, 0);
         mpq_set_d(x1, source.get_point().lon_);
         mpq_set_d(x2, target.get_point().lon_);
         mpq_set_d(y1, source.get_point().lat_);
         mpq_set_d(y2, target.get_point().lat_);   
         
         if (mpq_cmp(x1, x2) > 0)
         {
            mpq_swap(x1, x2);
            mpq_swap(y1, y2);
         }
         //lat,lon aligned to grid
         mpq_t y_min, y_max, x_min, x_max;
         mpq_inits(y_min, y_max, x_min, x_max, 0);
         cell_id_t id1 = get_cell_id<d>(y1, x1);
         cell_id_t id2 = get_cell_id<d>(y2, x2);
         
         mpq_t lat11, lon11, lat12, lon12; //id1_rect
         mpq_inits(lat11, lon11, lat12, lon12, 0);
         mpq_t lat21, lon21, lat22, lon22; //id2_rect
         mpq_inits(lat21, lon21, lat22, lon22, 0);

         get_cell_rect_hp<d>(id1, lat11, lon11, lat12, lon12);
         get_cell_rect_hp<d>(id2, lat21, lon21, lat22, lon22);
         
         mpq_set(y_min, lat11);
         mpq_set(y_max, lat22);
         mpq_set(x_min, lon11);
         mpq_set(x_max, lon22);
         
         if (mpq_cmp(y1, y2) > 0)
         {
            mpq_set(y_min, lat12);
            mpq_set(y_max, lat21);
            mpq_set(x_min, lon11);
            mpq_set(x_max, lon22);
         }
         mpq_t x, y;
         mpq_inits(x, y, 0);
         mpq_set(x, x_min);
         mpq_set(y, y_min);
         mpq_t slope; //ctg
         mpq_init(slope);
         mpq_t diff_x, diff_y;
         mpq_inits(diff_x, diff_y, 0);
         if (!mpq_equal(y1, y2))
         {
            mpq_sub(diff_x, x2, x1);
            mpq_sub(diff_y, y2, y1);
            mpq_div(slope, diff_x, diff_y);
         }
         
         mpq_t temp1, temp2, x_;
         mpq_inits(temp1, temp2, x_, 0);
         
         if (mpq_equal(y1, y2))
         {
            while(mpq_cmp(x, x_max) < 0)
            {
               mpq_add(temp1, x, little);
               cell_id_t new_cell_id = get_cell_id<d>(y2, temp1);
               cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
               mpq_set(temp1, x);
               mpq_add(x, temp1, grad_per_cell);
            }
         }
         else
         {
            if ( mpq_cmp(y_min, y_max) <= 0) 
            {
              while (mpq_cmp(y, y2) < 0) 
              {
                mpq_add(temp1, y, grad_per_cell);
                mpq_sub(temp2, temp1, y1);
                mpq_mul(temp1, slope, temp2);
                mpq_add(x_, temp1, x1);
                //x_ = slope * (y + grad_per_cell - y1) + x1;
                //x_ - x coord of intersection with current horizontal axis
                do
                {
                  mpq_add(temp1, y, little);
                  mpq_add(temp2, x, little);
                  cell_id_t new_cell_id = get_cell_id<d>(temp1, temp2);
                  cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
                  /*if (new_cell_id.first == 62981 && new_cell_id.second == 45066 && d == d2)
                  {
                     std::cout << "slope " << mpq_get_d (slope) << " " << mpq_get_str(0, 10, slope) << std::endl;
                     std::cout << "x " << mpq_get_d (x) << " " << mpq_get_str(0, 10, x) << std::endl;
                     std::cout << "x_ " << mpq_get_d (x_) << " " << mpq_get_str(0, 10, x_) << std::endl;
                     std::cout << "x_min " << mpq_get_d (x_min) << " " << mpq_get_str(0, 10, x_min) << std::endl;
                     std::cout << "x_max " << mpq_get_d (x_max) << " " << mpq_get_str(0, 10, x_max) << std::endl;
                     std::cout << "y " << mpq_get_d (y) << " " << mpq_get_str(0, 10, y) << std::endl;
                     std::cout << "y_min " << mpq_get_d (y_min) << " " << mpq_get_str(0, 10, y_min) << std::endl;
                     std::cout << "y_max " << mpq_get_d (y_max) << " " << mpq_get_str(0, 10, y_max) << std::endl;
                     int a = 0;
                  }*/
                  mpq_add(temp1, x, grad_per_cell);
                  mpq_set(x, temp1);
                }
                while ( (mpq_cmp(x, x_) < 0) && (mpq_cmp(x, x_max) < 0));
                mpq_sub(temp1, x, grad_per_cell);
                mpq_set(x, temp1);
                mpq_add(temp1, y, grad_per_cell);
                mpq_set(y, temp1);
              }
            }
            if (mpq_cmp(y_min, y_max) > 0)
            {
              while (mpq_cmp(y, y2) > 0) 
              {
                mpq_sub(temp1, y, grad_per_cell);
                mpq_sub(temp2, temp1, y1);
                mpq_mul(temp1, slope, temp2);
                mpq_add(x_, temp1, x1);
                //slope * (y - grad_per_cell - y1) + x1;
                //x_ - x coord of intersection with current horizontal axis
                do
                {
                  mpq_sub(temp1, y, little);
                  mpq_add(temp2, x, little);
                  cell_id_t new_cell_id = get_cell_id<d>(temp1, temp2);
                  cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
                  /*if (new_cell_id.first == 62981 && new_cell_id.second == 45066 && d == d2)
                  {
                     std::cout << "slope " << mpq_get_d (slope) << " " << mpq_get_str(0, 10, slope) << std::endl;
                     std::cout << "x " << mpq_get_d (x) << " " << mpq_get_str(0, 10, x) << std::endl;
                     std::cout << "x_ " << mpq_get_d (x_) << " " << mpq_get_str(0, 10, x_) << std::endl;
                     std::cout << "x_min " << mpq_get_d (x_min) << " " << mpq_get_str(0, 10, x_min) << std::endl;
                     std::cout << "x_max " << mpq_get_d (x_max) << " " << mpq_get_str(0, 10, x_max) << std::endl;
                     std::cout << "y " << mpq_get_d (y) << " " << mpq_get_str(0, 10, y) << std::endl;
                     std::cout << "y_min " << mpq_get_d (y_min) << " " << mpq_get_str(0, 10, y_min) << std::endl;
                     std::cout << "y_max " << mpq_get_d (y_max) << " " << mpq_get_str(0, 10, y_max) << std::endl;
                     int a = 0;
                  }*/
                  mpq_add(temp1, x, grad_per_cell);
                  mpq_set(x, temp1);
                }                
                while ((mpq_cmp(x, x_) < 0) && (mpq_cmp(x, x_max) < 0));
                mpq_sub(temp1, x, grad_per_cell);
                mpq_set(x, temp1);
                mpq_sub(temp1, y, grad_per_cell);
                mpq_set(y, temp1);
              }
            }
         }   
         mpq_clear(grad_per_cell);
         mpq_clear(little);
         mpq_clears(x1, x2, y1, y2, 0);
         mpq_clears(x, y, 0);
         mpq_clear(slope);
         mpq_clears(diff_x, diff_y, 0);
         mpq_clears(temp1, temp2, x_, 0);
         mpq_clears(lat21, lon21, lat22, lon22, 0);
         mpq_clears(lat11, lon11, lat12, lon12, 0);
      }
      
      template<>
      inline box_t graph_metadata_t::devide_box<1, 1>(box_t box)
      {
         size_t half_len_x = abs((int)box.second.first - (int)box.first.first) / 2;
         size_t half_len_y = abs((int)box.second.second - (int)box.first.second) / 2;
         return box_t( cell_id_t(box.first.first, box.second.second - half_len_y), 
                       cell_id_t(box.first.first + half_len_x, box.second.second) );
      }
      
      template<>
      inline box_t graph_metadata_t::devide_box<2, 1>(box_t box)
      {
         size_t half_len_x = abs((int)box.second.first - (int)box.first.first) / 2;
         size_t half_len_y = abs((int)box.second.second - (int)box.first.second) / 2;
         return box_t( cell_id_t(box.second.first - half_len_x, box.second.second - half_len_y), 
                       cell_id_t(box.second.first, box.second.second) );
      }
      
      template<>
      inline box_t graph_metadata_t::devide_box<2, 2>(box_t box)
      {
         size_t half_len_x = abs((int)box.second.first - (int)box.first.first) / 2;
         size_t half_len_y = abs((int)box.second.second - (int)box.first.second) / 2;
         return box_t( cell_id_t(box.second.first - half_len_x, box.first.second), 
                       cell_id_t(box.second.first, box.first.second + half_len_y) );
      }
      
      //To get best precision of this algorithm
      //it need version which fully works with gmp
      //and switching to this version if we have scal_mult == 0
      //now it is partly implemented.
      //Works with scalara multypliction (dot) of normal vector to edge 
      //and vectors to corners.   
      //Need projection to work well cause uses dot.
      //projection not implemented   
      template<delta d>
      inline void graph_metadata_t::rasterize_edge2(edge_t const & edge)
      {
         cell_edge_map_t* cur_d_edge_map = &get_cell_edge_map<d>(); //old code was with pointers
         lat_lon_t grad_per_cell = grad_per_cell_traits_t<d>::value();
        
         vertex_t const & source = (*graph_)[edge.get_source_vertex_id()];//0x00000110
         vertex_t const & target = (*graph_)[edge.get_target_vertex_id()];//0x00000111

         lat_lon_t x1 = source.get_point().lon_;
         lat_lon_t y1 = source.get_point().lat_;
         lat_lon_t x2 = target.get_point().lon_;
         lat_lon_t y2 = target.get_point().lat_;  
         
         #ifdef RASTER_TEST
         if ( abs(x1) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(x2) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(y1) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 || 
              abs(y2) <= std::numeric_limits<lat_lon_t>::epsilon() * 10 )
            return; //take out garbage
          #endif
         
         if (x1 > x2)
         {
            std::swap(x1, x2);
            std::swap(y1, y2);
         }
         
         //bounding box
         //gmp high precision
         //mpq_t mp_y1, mp_x1, mp_y2, mp_x2;
         //mpq_inits(mp_y1, mp_x1, mp_y2, mp_x2, 0);
         //mpq_set_d(mp_y1, y1);
         //mpq_set_d(mp_x1, x1);
         //mpq_set_d(mp_y2, y2);
         //mpq_set_d(mp_x2, x2);        
         //
         //cell_id_t id1 = get_cell_id<d>(mp_y1, mp_x1);
         //cell_id_t id2 = get_cell_id<d>(mp_y2, mp_x2);
         //mpq_clears(mp_y1, mp_x1, mp_y2, mp_x2, 0);
         
         cell_id_t id1 = get_cell_id<d>(point_t(y1, x1));
         cell_id_t id2 = get_cell_id<d>(point_t(y2, x2));
         
         box_t bounding_box;
         if (id1.second > id2.second)
            bounding_box = box_t(cell_id_t(id1.first, id2.second), cell_id_t(id2.first, id1.second));
         else
            bounding_box = box_t(cell_id_t(id1.first, id1.second), cell_id_t(id2.first, id2.second));
         
         std::queue<box_t> work_list;
         work_list.push(bounding_box);
         while(!work_list.empty())
         {
            box_t cur_box = work_list.front();
            work_list.pop();
            //box_pointxy;               
            point_t box_point12(get_cell_rect<d>(cur_box.first).first);
            point_t box_point21(get_cell_rect<d>(cur_box.second).second);
            point_t box_point11(box_point21.lat_, box_point12.lon_);
            point_t box_point22(box_point12.lat_, box_point21.lon_);
            
            //get vectors relative to x1,y1 point
            box_point12.lat_ -= y1;
            box_point12.lon_ -= x1;
            box_point21.lat_ -= y1;
            box_point21.lon_ -= x1;
            box_point11.lat_ -= y1;
            box_point11.lon_ -= x1;
            box_point22.lat_ -= y1;
            box_point22.lon_ -= x1;
           
            //vector of normal from point x1,y1
            //point_t norm_vect(y2, x2);
            point_t norm_vect(0, 1);
            norm_vect.lat_ = -(norm_vect.lon_ * (x2 - x1)) / (y2 - y1);
            if (y2 == y1)
            {
               norm_vect.lon_ = 0;
               norm_vect.lat_ = 1;
            }
            
            //check signs of scalar multyplication
            int scal_11_sgn = math::scalar_mult_sgn(norm_vect, box_point11);
            int scal_12_sgn = math::scalar_mult_sgn(norm_vect, box_point12);
            int scal_21_sgn = math::scalar_mult_sgn(norm_vect, box_point21);
            int scal_22_sgn = math::scalar_mult_sgn(norm_vect, box_point22);
            if ( 
               !((scal_11_sgn > 0) && (scal_12_sgn > 0) && (scal_21_sgn > 0) && (scal_22_sgn > 0)) &&
               !((scal_11_sgn < 0) && (scal_12_sgn < 0) && (scal_21_sgn < 0) && (scal_22_sgn < 0))
               )
            {
               //if width and height of box is 1, then it is graph's pixel 
               if ( abs((int)cur_box.first.first - (int)cur_box.second.first) == 0 && 
                    abs((int)cur_box.first.second - (int)cur_box.second.second) == 0)
               {
                  cell_id_t new_cell_id(cur_box.first.first, cur_box.first.second);
                  cur_d_edge_map->insert(std::make_pair(new_cell_id, edge));
               }
               else
               {
                  work_list.push(devide_box<1, 2>(cur_box));
                  work_list.push(devide_box<1, 1>(cur_box));
                  work_list.push(devide_box<2, 1>(cur_box));
                  work_list.push(devide_box<2, 2>(cur_box));
               }
            }
         }
      }
      
      template<delta d>
      inline cell_edge_map_t& graph_metadata_t::get_cell_edge_map()
      {
         struct ERROR_CELL_EDGE_MAP_IS_UNDEFINED_FOR_THIS_DELTA;
         ERROR_CELL_EDGE_MAP_IS_UNDEFINED_FOR_THIS_DELTA ERROR;
      }
      
      template<>
      inline cell_edge_map_t& graph_metadata_t::get_cell_edge_map<d1>()
      {
         return cell_edge_map_d1_;
      }
      
      template<>
      inline cell_edge_map_t& graph_metadata_t::get_cell_edge_map<d2>()
      {
         return cell_edge_map_d2_;
      }
      
      template<>
      inline cell_edge_map_t& graph_metadata_t::get_cell_edge_map<d4>()
      {
         return cell_edge_map_d4_;
      }

      template<delta d>
      inline cell_edge_map_t const & graph_metadata_t::get_cell_edge_map() const
      {
         struct ERROR_CELL_EDGE_MAP_IS_UNDEFINED_FOR_THIS_DELTA;
         ERROR_CELL_EDGE_MAP_IS_UNDEFINED_FOR_THIS_DELTA ERROR;
      }
      
      template<>
      inline cell_edge_map_t const & graph_metadata_t::get_cell_edge_map<d1>() const
      {
         return cell_edge_map_d1_;
      }
      
      template<>
      inline cell_edge_map_t const & graph_metadata_t::get_cell_edge_map<d2>() const
      {
         return cell_edge_map_d2_;
      }
      
      template<>
      inline cell_edge_map_t const & graph_metadata_t::get_cell_edge_map<d4>() const
      {
         return cell_edge_map_d4_;
      }
      
      template<delta d> 
      inline cell_edge_iterator_pair_t graph_metadata_t::cell_edges(cell_id_t id) const
      {
         return get_cell_edge_map<d>().equal_range(id);
      }

      inline cell_vertex_iterator_pair_t graph_metadata_t::cell_vertices(cell_id_t id) const
      {
         return cell_vertex_map_d1_.equal_range(id);
      }
      
      template<delta d> 
      inline cell_edge_iterator_pair_t graph_metadata_t::get_cells() const
      {
         return cell_edge_iterator_pair_t(get_cell_edge_map<d>().begin(),
                                          get_cell_edge_map<d>().end());
      } 
      
      //devides any rectangular box on four parts
      //params are: 12, 11, 21, 22
      template<int corner_x, int corner_y>
      inline box_t graph_metadata_t::devide_box(box_t)
      {
         struct ERR_UNDEFINED_DIVISION_OF_BOX_BY_GIVEN_TEMPLATE_ARGS;
         ERR_UNDEFINED_DIVISION_OF_BOX_BY_GIVEN_TEMPLATE_ARGS err;
      }
      
      template<>
      inline box_t graph_metadata_t::devide_box<1, 2>(box_t box)
      {         
         size_t half_len_x = abs((int)box.second.first - (int)box.first.first) / 2;
         size_t half_len_y = abs((int)box.second.second - (int)box.first.second) / 2;
         return box_t( box.first, cell_id_t(box.first.first + half_len_x, box.first.second + half_len_y) );
      }
      
      template<delta d>
      inline cell_id_t graph_metadata_t::max_cell_id()
      {
         //minus 1/2 from grad_per_cell to make point for which id is max be in center of max cell
         return get_cell_id<d, false>(point_t(90 - grad_per_cell_traits_t<d>::value() / 2, 
                                       180 - grad_per_cell_traits_t<d>::value() / 2)
                                       );
      }
   }
}
